package es.uji.viselab.experiment.control;

import es.uji.viselab.experiment.ExperimentParameters;
import es.uji.viselab.image.source.ImageSource;
import es.uji.viselab.math.Matrix4;
import es.uji.viselab.math.Vector3;
import es.uji.viselab.robot.RobotException;
import es.uji.viselab.visualservoing.VisualServoingTask;

public class PBVS_Control extends ExperimentControl {

	public PBVS_Control(ExperimentParameters ep) {
		super(ep);
	}

	@Override
	public void start() {
		
		imageSource.compute(robot);

		double[] velocitiesCameraFrame = visualServoing.compute(imageSource);

		// They should be transformed to world (robot) coordinate system
		double[] velocities = eyeOnHandCameraCoordinatesToRobotCoordinates(velocitiesCameraFrame);
		System.out.println("Displacement end-effector requested: " + velocities[0] + ", "
				+ velocities[1] + ", " + velocities[2]);
		try {
			robot.move(velocities);
		} catch (RobotException e) {
			e.printStackTrace();
		}
	}

	
}
